#include <stdio.h>
#include <unistd.h>
#include <stdlib.h>
#include "../drv/drv_i2c.h"
#include "stlis331dlh.h"

//I2C address 
#define STLIS331DLH_ADDRESS		0x18

#define LIS331DLH_SENSIVITY_2G         	0.000061036       // +-2g / 0xFFFF
#define LIS331DLH_SENSIVITY_4G         	0.000122072       // +-4g / 0xFFFF
#define LIS331DLH_SENSIVITY_8G         	0.000244144       // +-8g / 0xFFFF
#define LIS331DLH_1G_2G        		16383.75      	
#define LIS331DLH_1G_4G         	8191.875      	
#define LIS331DLH_1G_8G         	4095.937  
   	

//calibration data
S_Acc acc_calib;
int acc_calibrate = 0;


void STLIS331DLHInit( int fd )
{
	printf("STLIS331DLH Init \n");
	drv_i2cSetDevice(fd, STLIS331DLH_ADDRESS );
        drv_i2cWriteByte(fd, STLIS331DLH_REG1, STLIS331DLH_REG1_400);
        drv_i2cWriteByte(fd, STLIS331DLH_REG4, STLIS331DLH_2G);

	acc_calib.x = 0;
	acc_calib.y = 0;
	acc_calib.z = 0;

	acc_calibrate = 0;
}

void STLIS331DLHCalibrate( int fd )
{
	printf("STLIS331DLH Calibrate...");
	fflush(stdout);
	float acc_sum[ 3 ] = {0,0,0};
	S_Acc acc_read;
	int i;
	int div = 20;
	for(i=0 ; i < div ; i++)
	{
		STLIS331DLHRead(fd , &acc_read);
		acc_sum[ 0 ] = acc_sum[ 0 ] + acc_read.x;
		acc_sum[ 1 ] = acc_sum[ 1 ] + acc_read.y;
		acc_sum[ 2 ] = acc_sum[ 2 ] + acc_read.z;
	}
	acc_calib.x = (float)(acc_sum[ 0 ] / div);
	acc_calib.y = (float)(acc_sum[ 1 ] / div);
	acc_calib.z = (float)(acc_sum[ 2 ] / div);
	acc_calib.z = acc_calib.z - LIS331DLH_1G_2G;
	acc_calibrate = 1;

	printf("\rSTLIS331DLH Calibrate   \n");
}

void STLIS331DLHRead( int fd , S_Acc * acc)
{

	drv_i2cSetDevice(fd, STLIS331DLH_ADDRESS);
	unsigned char lsb = drv_i2cReadByte(fd, STLIS331DLH_ACCX_L);
	unsigned char msb = drv_i2cReadByte(fd, STLIS331DLH_ACCX_H);
	acc->x = (float)(signed short)((unsigned short)msb << 8) + lsb ;
	
	lsb = drv_i2cReadByte(fd, STLIS331DLH_ACCY_L);
	msb = drv_i2cReadByte(fd, STLIS331DLH_ACCY_H);
	acc->y = (float)(signed short)((unsigned short)msb << 8) + lsb ;
	
	lsb = drv_i2cReadByte(fd, STLIS331DLH_ACCZ_L);
	msb = drv_i2cReadByte(fd, STLIS331DLH_ACCZ_H);
	acc->z = (float)(signed short)((unsigned short)msb << 8) + lsb ;

	if(acc_calibrate == 1)
	{
		acc->x = acc->x - acc_calib.x;
		acc->y = acc->y - acc_calib.y;
		acc->z = acc->z - acc_calib.z;

		acc->x = acc->x * LIS331DLH_SENSIVITY_2G;
		acc->y = acc->y * LIS331DLH_SENSIVITY_2G;
		acc->z = acc->z * LIS331DLH_SENSIVITY_2G;
	}


}

